#!/usr/bin/env python
import rospy
from std_msgs.msg import Bool
from geometry_msgs.msg import Pose
from moveit_commander import MoveGroupCommander
import moveit_commander
import sys
import numpy as np
from tf.transformations import quaternion_matrix

import time


# 回调函数，用于接收 adjusted_pose_left
left_pose = None
def left_pose_callback(msg):
    global left_pose
    left_pose = msg

# 回调函数，用于接收 adjusted_pose_right
right_pose = None
# 任务标志，默认在在grasp和release之间循环
task = "grasp"
def right_pose_callback(msg):
    global right_pose ,task
    right_pose = msg
    rospy.loginfo("Received both left and right poses, starting movement.")

    quaternion = [msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w ]

    rotation_matrix = quaternion_matrix(quaternion)
    
    local_offset = np.array([0.15, 0.0, 0.0, 1.0])

    world_offset = np.dot(rotation_matrix, local_offset)

    #print("right_pose",right_pose)
    rate = rospy.Rate(1)
    if task == "grasp":
        print(task)
        msg_hand_control1 = Bool()
        msg_hand_control1.data = True
        #hand_control_publisher.publish(msg_hand_control1)
        #hand_control_publisher.publish(msg_hand_control1)
        #rospy.loginfo(f"Publish True at {rospy.get_time()}") #打印时间戳
        #rospy.sleep(2)
        #rospy.loginfo(f"Published True at {rospy.get_time()}") #打印时间戳
        # 移动右手臂到调整后的位姿
        
        right_pose.position.x = right_pose.position.x + world_offset[0]
        right_pose.position.y = right_pose.position.y + world_offset[1]
        right_pose.position.z = right_pose.position.z + world_offset[2] #+ 0.15
        '''
        right_pose.position.x = 0.335
        right_pose.position.y = -0.383
        right_pose.position.z = 0.953
        right_pose.orientation.x=0.227
        right_pose.orientation.y=-0.600
        right_pose.orientation.z=0.670
        right_pose.orientation.w=0.374
        '''

        move_to_pose(right_pose, "Right_Arm_Link7")

        # 第二步 接近杯子
        #right_pose.position.x = right_pose.position.x - world_offset[0]
        #right_pose.position.y = right_pose.position.y - world_offset[1]
        #right_pose.position.z = right_pose.position.z - world_offset[2] #+ 0.15
        #move_to_pose(right_pose, "Right_Arm_Link7")
        #msg_hand_control2 = Bool()
        #msg_hand_control2.data = False
        #hand_control_publisher.publish(msg_hand_control2)
        #hand_control_publisher.publish(msg_hand_control2)
        #rospy.loginfo(f"Publish False at {rospy.get_time()}") #打印时间戳
        #rospy.sleep(2)
        #rospy.loginfo(f"Published False at {rospy.get_time()}") #打印时间戳
        task = "release"

    elif task == "release":
        print(task)
        #msg_hand_control = Bool()
        #msg_hand_control.data = True
        #hand_control_publisher.publish(msg_hand_control)
        #rospy.sleep(4)
        # 移动右手臂到调整后的位姿
        #right_pose.position.z = right_pose.position.z + 0.15
        #move_to_pose(right_pose, "Right_Arm_Link7")
        task = "grasp"

# 订阅 adjusted_pose_left 和 adjusted_pose_right 话题
#rospy.Subscriber('/adjusted_pose_left', Pose, left_pose_callback)



def move_to_pose(pose, link_name):
    """
    移动手臂到指定位姿。
    :param pose: Pose 消息，目标位姿
    :param link_name: 要移动的链接名称
    """
    global move_group
    print("adjusted_right_pose",pose)
    print(move_group.get_current_pose().pose)


    #move_group.set_pose_target(move_group.get_current_pose().pose)
    move_group.set_pose_target(pose)
    plan,traj,planning_time,error_code = move_group.plan()

    print(planning_time)

    success = move_group.execute(traj, wait=True)

    move_group.stop()
    #move_group.clear_pose_target(pose)

    if success:
        rospy.loginfo(f"Successfully moved {link_name} to the target pose.")
    else:
        rospy.logwarn(f"Failed to move {link_name} to the target pose.")
        



# 主循环
if __name__ == "__main__":
    # 初始化 ROS 节点
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('move_arm_to_poses', anonymous=True)

    # MoveIt 初始化
    move_group = MoveGroupCommander("right_arm")
    move_group.set_end_effector_link("right_tool_link")
    move_group.set_planning_time(0.1) # 0.1s 的规划用时限制
    move_group.set_max_velocity_scaling_factor(0.8)
    move_group.set_max_acceleration_scaling_factor(0.8)
    print(move_group.get_planning_frame())

    hand_control_publisher = rospy.Publisher("/hand_control_topic", Bool, queue_size=1)
    rospy.sleep(2)
    rospy.Subscriber('/right_grasp_pose', Pose, right_pose_callback, queue_size=1)
    rospy.spin()
'''
rate = rospy.Rate(10)  # 10 Hz
while not rospy.is_shutdown():
    if  right_pose:
        rospy.loginfo("Received both left and right poses, starting movement.")

        # 调整右手臂的位姿以考虑夹爪
        adjusted_right_pose = adjust_pose_for_gripper(right_pose, 0.20)

        # 移动左手臂到 left_pose
        #move_to_pose(left_pose, "Left_Arm_Link7")

        # 移动右手臂到调整后的位姿
        move_to_pose(adjusted_right_pose, "Right_Arm_Link7")

        # 清除位姿，避免重复移动
        right_pose = None

    rate.sleep()
'''